#include <iostream>
#include <string>
#include <sstream>
#include <fstream>
#include <iomanip>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <serial/serial.h>

#include <nav_msgs/Odometry.h>  
#include <tf/transform_broadcaster.h>

const double R2D = 180.0 / 3.1415926535;
const double D2R = 3.1415926535 / 180.0;

/**
 * @brief 数据类型转换联合体，用于float和字节数组间的转换
 */
typedef union
{
	float data;
	unsigned char data8[4];
} data_u;

int main(int argc, char** argv)
{
	ros::init(argc, argv, "read_imu_odom");
	ros::NodeHandle nh;
	ros::NodeHandle private_nh("~");

	time_t tt = time(NULL);
    struct tm* t = localtime(&tt);

    std::stringstream filename;
	// 生成存放结果的目录 根目录/数据序号_yyyymmdd_hhmmss
	auto generate_file_path = [](const std::string& path) {
		auto get_two_digit = [](int n) {
			std::stringstream ss;
			ss << std::setw(2) << std::setfill('0') << n;
			return ss.str();
		};
		time_t tt = time(NULL);
		struct tm* t = localtime(&tt);
		std::stringstream ss;
		ss << path << "data_" << t->tm_year + 1900
				<< get_two_digit(t->tm_mon + 1) 
				<< get_two_digit(t->tm_mday) << "_" 
				<< get_two_digit(t->tm_hour) 
				<< get_two_digit(t->tm_min) 
				<< get_two_digit(t->tm_sec)
				<< ".txt";
		return ss.str();
	};
	auto save_dir = generate_file_path("/home/nuc/workspace/ai-robot-plus/catkin_ws/src/xrobot/data/");
	std::cout << "save_dir: " << save_dir << std::endl;
    std::ofstream output_file_s;
    output_file_s.open(save_dir);
    output_file_s.precision(15);

    auto startTime = ros::Time::now();

	bool is_auto_start = false;
	private_nh.param("is_auto_start", is_auto_start, false);

	// 如果要让该节点开机自启，则需要延时等待串口得到权限
	if(is_auto_start)
		ros::Duration(10).sleep();

	serial::Serial ser;
	std::string serial_port;
	int serial_baudrate;
	private_nh.param("serial_port", serial_port, std::string("/dev/ttyACM0"));
	ROS_INFO("Port: %s", serial_port.c_str());
	private_nh.param("serial_baudrate", serial_baudrate, 460800);
	ROS_INFO("Baudrate: %d", serial_baudrate);
	// 打开串口
	try
	{
		ser.setPort(serial_port);
		ser.setBaudrate(serial_baudrate);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		ser.setTimeout(to);
		ser.open();
	}
	catch(serial::IOException &e)
	{
		ROS_INFO_STREAM("Failed to open port ");
		return -1;
	}
	ROS_INFO_STREAM("Succeed to open port" );

	// 设置读取频率，读取频率 > 数据包长度 * 上传频率
	ros::Rate loop_rate(100000); 

	int state = 0;
	unsigned char data_receive[100] = {0};
    int data_cnt = 0;
	int last_seq = 0;

	// clear buffer
	ser.flushInput();
	int cnt = 0;
	while(cnt++ < 100000){
		if(ser.available())
			ser.read(ser.available());
	}

    while(ros::ok())
	{
		if(ser.available())
		{
            uint8_t buffer = 0;
			ser.read(&buffer, 1);

			if(state == 0 && buffer == 0xAA)
			{
				state++;
			}
			else if(state == 1 && buffer == 0x55)
			{
				state++;
			}
			else if(state == 2)
			{
				data_receive[data_cnt++]=buffer;
				if(data_cnt == 25)
				{
					
					/* 进行数据校验 */
					uint8_t checkout = 0;
					for(int k = 0; k < data_cnt - 1; k++)
					{
						checkout += data_receive[k];
					}
					if(checkout == data_receive[data_cnt - 1]) // 串口接收到的最后一个字节是校验码 
					{
						/* 校验通过，进行解码 */
						uint32_t seq = *((uint32_t*)data_receive);
						float gyro_rps[3];
						float accel_mpss[3];
						int16_t gyro_raw[3];
						int16_t accel_raw[3];
						for(int i = 0; i < 3; i++){
							gyro_raw[i] = *((int16_t*)(data_receive + 4) + i);
							accel_raw[i] = *((int16_t*)(data_receive + 4) + i + 3);
							gyro_rps[i] = gyro_raw[i] * 0.0010650782; // ±2000dps (4000 * 3.1415926535 / 180 / 65536)
							accel_mpss[i] = accel_raw[i] * 0.004790; // ±16g (32 * 9.81 / 65536) 
						}

						int16_t encoder_incre[4];
						for(int i = 0; i < 4; i++){
							encoder_incre[i] = *((int16_t*)(data_receive + 4) + i + 6);
						}
						output_file_s   << seq << " "
										<< gyro_raw[0] << " " << gyro_raw[1] << " " << gyro_raw[2] << " "
										<< accel_raw[0] << " " << accel_raw[1] << " " << accel_raw[2] << " "
										// << gyro_rps[0] << " " << gyro_rps[1] << " " << gyro_rps[2] << " " 
										// << accel_mpss[0] << " " << accel_mpss[1] << " " << accel_mpss[2] << " "
										<< encoder_incre[0] << " " << encoder_incre[1] << " " << encoder_incre[2] << " " << encoder_incre[3] << " "
										<< std::endl;

						if(seq - last_seq != 1)
						{
							ROS_INFO("lost! seq: %d, last_seq: %d", seq, last_seq);
						}
						last_seq = seq;

						// ROS_INFO("seq: %d, linear_x: %f, linear_y: %f, angular_z: %f", seq, accel_mpss[0], accel_mpss[1], accel_mpss[2]);
						// ROS_INFO("seq: %d, encoder_incre: %d, %d, %d, %d", seq, encoder_incre[0], encoder_incre[1], encoder_incre[2], encoder_incre[3]);
						
					}
					
					data_cnt = 0;
					state = 0;
				}
			}
			else state = 0;
		}
		loop_rate.sleep();
	}
	// 关闭串口
	ser.close();

	return 0; 
}